#include "usr_inc.h"

/* 定义ID为4的2006电机实例 */
Dji_t dji4;
Pid_t velPid4;
Pid_t posPid4;
int32_t baseCnt4;

/**
 * @brief 初始化所有运球相关的实例
 */
void ALL_DRIBBLE_Init(void)
{
    DjiInit_t djiInit = {0};
    PidInit_t pidInit = {0};

    djiInit.motorID = 4;
    djiInit.hcan = &hcan2;
    DJI_Init(&dji4, &djiInit);

    pidInit.kp = 40.0;
    pidInit.ki = 0.5;
    pidInit.kd = 0.0;
    pidInit.form = PID_POSITIONAL_FORM;
    pidInit.integralLimit = 6000 / 3;
    pidInit.maxOutput = 6000;
    pidInit.minOutput = -6000;
    PID_Init(&velPid4, &pidInit);

    pidInit.kp = 450.0;
    pidInit.ki = 0.0;
    pidInit.kd = 0.0;
    pidInit.form = PID_POSITIONAL_FORM;
    pidInit.integralLimit = 240 / 3;
    pidInit.maxOutput = 240;
    pidInit.minOutput = -240;
    PID_Init(&posPid4, &pidInit);
}

void StartDribbleUpdate(void *argument)
{
    FunctionalState extendFlag = DISABLE;

    for (;;)
    {
        DribbleCmdMsg_t cmd;
        xQueueReceive(dribbleCmdQueueHandle, &cmd, portMAX_DELAY);

        if (cmd == DRIBBLE_EXTEND)
        {
            PID_SetTarget(&posPid4, 2.45);
            extendFlag = ENABLE;
        }
        else if (cmd == DRIBBLE_RETRACT)
        {
            PID_SetTarget(&posPid4, 0.0);
            extendFlag = DISABLE;
        }
        else if (cmd == DRIBBLE_DRAW)
        {
            HAL_GPIO_WritePin(VALVE2_GPIO_Port, VALVE2_Pin, GPIO_PIN_SET);
        }
        else if (cmd == DRIBBLE_FREE)
        {
            /* 泄气 */
            HAL_GPIO_WritePin(VALVE2_GPIO_Port, VALVE2_Pin, GPIO_PIN_RESET);
            HAL_GPIO_WritePin(VALVE3_GPIO_Port, VALVE3_Pin, GPIO_PIN_SET);
            vTaskDelay(pdMS_TO_TICKS(200));

            /* 回 */
            HAL_GPIO_WritePin(VALVE1_GPIO_Port, VALVE1_Pin, GPIO_PIN_RESET);
            vTaskDelay(pdMS_TO_TICKS(300));

            /* 恢复 */
            HAL_GPIO_WritePin(VALVE3_GPIO_Port, VALVE3_Pin, GPIO_PIN_RESET);
        }
        else if (cmd == DRIBBLE_HOLD)
        {
            if (extendFlag == ENABLE)
            {
                HAL_GPIO_WritePin(VALVE4_GPIO_Port, VALVE4_Pin, GPIO_PIN_RESET);
                HAL_GPIO_WritePin(VALVE1_GPIO_Port, VALVE1_Pin, GPIO_PIN_SET);
            }
        }
        else if (cmd == DRIBBLE_PAT)
        {
            if (extendFlag == ENABLE)
            {
                /* 拍 */
                HAL_GPIO_WritePin(VALVE4_GPIO_Port, VALVE4_Pin, GPIO_PIN_SET);
                HAL_GPIO_WritePin(VALVE3_GPIO_Port, VALVE3_Pin, GPIO_PIN_SET);
                vTaskDelay(pdMS_TO_TICKS(400));

                /* 回 */
                HAL_GPIO_WritePin(VALVE1_GPIO_Port, VALVE1_Pin, GPIO_PIN_RESET);
                HAL_GPIO_WritePin(VALVE4_GPIO_Port, VALVE4_Pin, GPIO_PIN_RESET);
                HAL_GPIO_WritePin(VALVE3_GPIO_Port, VALVE3_Pin, GPIO_PIN_RESET);
                vTaskDelay(pdMS_TO_TICKS(350));

                /* 稳 */
                HAL_GPIO_WritePin(VALVE1_GPIO_Port, VALVE1_Pin, GPIO_PIN_SET);
            }
        }
        else if (cmd == DRIBBLE_RETRY)
        {
            HAL_GPIO_WritePin(VALVE1_GPIO_Port, VALVE1_Pin, GPIO_PIN_RESET);
        }
    }
}

void StartDji4Exe(void *argument)
{
    baseCnt4 = DJI_GetCnt(dji4InfoBoxHandle);

    TickType_t xLastWakeTime = xTaskGetTickCount();
    for (const int16_t period = 5; ;)
    {
        PID_Calculate(&posPid4, DJI_GetM2006Pos(dji4InfoBoxHandle, baseCnt4));
        PID_SetTarget(&velPid4, PID_GetOutput(&posPid4));
        PID_Calculate(&velPid4, DJI_GetM2006Rpm(dji4InfoBoxHandle));
        DJI_WriteCmdBuf(&dji4, PID_GetOutput(&velPid4), djiMutexHandle);

        vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(period));
    }
}

void StartDribblePrintf(void *argument)
{
    vTaskSuspend(NULL);

    for (const int16_t period = 30; ;)
    {
        // PidArgsMsg_t msg;
        // if (xQueueReceive(pidArgsQueueHandle, &msg, 5) == pdTRUE)
        // {
        //     PID_SetArgs(&posPid4, msg.kp, msg.ki, msg.kd);
        //     PID_SetTarget(&posPid4, msg.target);
        // }

        printf("%f, %f, %f, %f\r\n", DJI_GetM2006Pos(dji4InfoBoxHandle, baseCnt4), posPid4.target, DJI_GetM2006Rpm(dji4InfoBoxHandle) / 100.0 - 3, velPid4.target / 100.0 - 3);

        vTaskDelay(pdMS_TO_TICKS(period));
    }
}
